#include "COsmMap.h"

//#define DEBUG

using namespace std;

COsmMap::COsmMap()
{
    bounds = new COsmBounds();
}

bool
COsmMap::findPath(long int start, long int goal,vector<COsmNode > &path)
{
#ifdef DEBUG
    cout << "Searching path from id: " << nodes[start].id << " to id: " << nodes[goal].id << endl;
#endif
   
    unsigned long int nSize = nodes.size();
    long int index;
    CHeap heap(nSize);
    double g_score[nSize];
    double h_score[nSize];
    bool visited[nSize];
    long int predecessor[nSize];
    for(unsigned long int i = 0; i < nSize; i++)
    {
        h_score[i] = nodes[i].GetDistance(&nodes[goal]);
        g_score[i] = -1;
        visited[i] = false;
        predecessor[i] = -1;
    }

    // Add start node to heap
    index = start;
    g_score[index] = 0;
    predecessor[index] = index;
    heap.add(index,h_score[index]);

    index  = heap.getFirst();
    while(index >= 0)
    {
#ifdef DEBUG
        cout << "Exploring node with id: " << nodes[index].id << endl;
#endif
        // Mark node as visited
        visited[index] = true;
        // If this node is goal node then reconstruct path
        if(nodes[goal].id == nodes[index].id)
        {
#ifdef DEBUG
            cout << "Goal node reached" << endl;
#endif
            // Reconstruct path
            while(index != predecessor[index])
            {
                path.push_back(nodes[index]);
                index = predecessor[index];
            }
            path.push_back(nodes[index]);
            // Revert path
            COsmNode node;
            for(unsigned long int i = 0; i < path.size()/2; i++)
            {
                node = path[i];
                path[i] = path[path.size()-1-i];
                path[path.size()-1-i] = node;
            }
#ifdef DEBUG
            for(unsigned long int i = 0; i < path.size();i++)
            {
                cout << path[i].id << endl;
            }
#endif
            return true;
        }

        // Expand
        for(unsigned int n = 0; n < nodes[index].neighbours.size(); n++)
        {
            long int nindex = nodes[index].neighbours[n];
#ifdef DEBUG
            cout << "-->Exploring neighbour with id: " << nodes[nindex].id << endl;
#endif
            if(nindex < 0 || visited[nindex])
            {
#ifdef DEBUG
                cout << "---->This node was visited yet." << endl;
#endif
                continue;
            }
            double t_g_score = g_score[index]+nodes[nindex].GetDistance(&nodes[index]);
            // is in heap?
            if(g_score[nindex] < 0)
            {
#ifdef DEBUG
                cout << "---->Is not in heap. Add it." << endl;
#endif
                g_score[nindex] = t_g_score;
                predecessor[nindex] = index;
                heap.add(nindex,g_score[nindex]+h_score[nindex]);
            }
            else if(g_score[nindex] < t_g_score)
            {
#ifdef DEBUG
                cout << "---->Is in heap with worse cost." << endl;
#endif
                g_score[nindex] = t_g_score;
                predecessor[nindex] = index;
                heap.update(nindex,g_score[nindex]+h_score[nindex]);
            }
#ifdef DEBUG
            else
                cout << "---->Is in heap with better cost." << endl;
#endif
        }
        index = heap.getFirst();
    }
    return false;
}

long int COsmMap::getOsmNode(long int id)
{
    for(unsigned long int i=0;i < nodes.size();i++)
    {   
        if(nodes[i].id == id)
            return i;
    }
    return -1;
}

long int COsmMap::getOsmWay(long int id)
{
    for(unsigned long int i=0;i < ways.size();i++)
    {
        if(ways[i].id == id);
            return i;
    }
    return -1;
}

long int COsmMap::getOsmRelation(long int id)
{
    for(unsigned long int i=0;i < relations.size();i++)
    {
        if(relations[i].id == id)
            return i;
    }
    return -1;
}

void COsmMap::printMap()
{
    for(unsigned long int i = 0; i < nodes.size(); i++)
        nodes[i].Print();
}


void COsmMap::drawMap(CRawImage *im, double *lat, double *lon)
{
    int yc,base;

    // Draw map into image
    for(int y = 0; y < MAP_HEIGHT; y++)
    {
        yc = (MAP_HEIGHT-y)*im->width;
        for(int x = 0; x < MAP_WIDTH; x++)
        {
            base = 3*(yc+x);
            im->data[base] = drawBuffer[3*(y*MAP_WIDTH+x)];
            im->data[base + 1] = drawBuffer[3*(y*MAP_WIDTH+x) + 1];
            im->data[base + 2] = drawBuffer[3*(y*MAP_WIDTH+x) + 2];
        }
    }
    double delLon = bounds->maxlon - bounds->minlon;
    double delLat = bounds->maxlat - bounds->minlat;
    // Formate current position into drawBuffer coorinates
    if(*lat < bounds->minlat) *lat = bounds->minlat;
    if(*lat > bounds->maxlat) *lat = bounds->maxlat;    
    if(*lon < bounds->minlon) *lon = bounds->minlon;
    if(*lon > bounds->maxlon) *lon = bounds->maxlon;
    int cY = (int)(((latCalc*(*lat) - latCalc*bounds->minlat)/(latCalc*delLat))*(MAP_HEIGHT-10)+5.5);
    int cX = (int)(((lonCalc*(*lon) - lonCalc*bounds->minlon)/(lonCalc*delLon))*(MAP_WIDTH-10)+5.5);
    // Draw current position pointer into image
    drawPoint(cX,cY,255,255,255);
}

void COsmMap::predrawMap()
{
    int lastX;
    int lastY;
    int cX,cY;

    // Set background (RGB: 0,0,0)
    memset(drawBuffer,0,MAP_HEIGHT*MAP_WIDTH*3);
    cout << "Setting background" << endl;

    lastX = lastY = -1;
    double delLat = bounds->maxlat - bounds->minlat;
    double delLon = bounds->maxlon - bounds->minlon;

    // Draw ways
    for(unsigned long int w = 0; w < ways.size(); w++)
    {
        // Draw nodes
        for(unsigned long int n = 0; n < ways[w].nodes.size(); n++)
        {
            // Calculate node position in drawBuffer coordinates
            if(nodes[ways[w].nodes[n]].lat < bounds->minlat || nodes[ways[w].nodes[n]].lat > bounds->maxlat ||
               nodes[ways[w].nodes[n]].lon < bounds->minlon || nodes[ways[w].nodes[n]].lon > bounds->maxlon)
                continue;
            cY = (int)(((nodes[ways[w].nodes[n]].lat - bounds->minlat)/delLat)*(MAP_HEIGHT-10)+5.5);
            cX = (int)(((nodes[ways[w].nodes[n]].lon - bounds->minlon)/delLon)*(MAP_WIDTH-10)+5.5);
            
            // Draw node
            drawPoint(cX,cY,255,0,0);
            // Draw line between two nodes
            if(lastX >= 0)
            {
                drawLine(lastX,lastY,cX,cY,0,255,255);
            }
            // current position is next last position
            lastX = cX;
            lastY = cY;
        }
        lastX = lastY = -1;
    }
}

void COsmMap::drawPoint(int x, int y, int r, int g, int b)
{
    char point[] = {-1, 1,  0, 1,  1, 1,
                    -1, 0,  0, 0,  1, 0,
                    -1,-1,  0,-1,  1,-1};
    int pointSize = sizeof(point)/2;
    for(int i = 0; i < pointSize; i++)
    {
        int base = 3*(y+point[2*i+1])*MAP_WIDTH + x + point[2*i];
        if(base < 0 || (unsigned)base+2 > sizeof(drawBuffer))
            continue;
        drawBuffer[base] = r;
        drawBuffer[base+1] = g;
        drawBuffer[base+2] = b;
    }
}

void COsmMap::drawLine(int x1, int y1, int x2, int y2, int r, int g, int b)
{
    int dx = x2 - x1;
    int dy = y2 - y1;
    int stepx = 1;
    int stepy = 1;

    if(dx < 0)
    {
        dx = -dx;
        stepx = -stepx;
    }
    if(dy < 0)
    {
        dy = -dy;
        stepy = -stepy;
    }

    dx <<= 1;
    dy <<= 1;

    int base;
    if(dx > dy)
    {
        int frac = dy - (dx >> 1);
        while(x1 != x2)
        {
            if(frac >= 0)
            {
                y1 += stepy;
                frac -= dx;
            }
            x1 += stepx;
            frac += dy;
            base = 3*y1*MAP_WIDTH + x1;
            drawBuffer[base] = r;
            drawBuffer[base+1] = g;
            drawBuffer[base+2] = b;
        }
    }
    else
    {
        int frac = dx - (dy >> 1);
        while(y1 != y2)
        {
            if(frac >= 0)
            {
                x1 += stepx;
                frac -= dy;
            }
            y1 += stepy;
            frac += dx;
            base = 3*y1*MAP_WIDTH + x1;
            drawBuffer[base] = r;
            drawBuffer[base+1] = g;
            drawBuffer[base+2] = b;
        }
    }
}
